/*    

    bobbietalk - a visual programming environment for toy robotics

    Copyright (C) 2013  Roberto Tiella

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
package bobbietalk.simulator.world;

import javax.vecmath.Matrix3d;
import javax.vecmath.Vector3d;

/**
 *
 * @author roberto
 */
public abstract class CinematicWorldObject extends WorldObject {

    private static final Matrix3d ONE = new Matrix3d(new double[]{1.0, 0.0, 0.0, 0.0, 1, 0, 0, 0, 1});
    public Vector3d translationSpeed = new Vector3d();
    public Matrix3d rotationSpeed = new Matrix3d(ONE);
    private double dt = 0.1; // seconds

    public double getDt() {
        return dt;
    }

    public void setDt(double dt) {
        this.dt = dt;
    }

    public Vector3d getTranslationSpeed() {
        return translationSpeed;
    }

    public void setTranslationSpeed(Vector3d translationSpeed) {
        this.translationSpeed = translationSpeed;
    }

    public Matrix3d getRotationSpeed() {
        return rotationSpeed;
    }

    public void setRotationSpeed(Matrix3d rotationSpeed) {
        this.rotationSpeed = rotationSpeed;
    }

    public void updatePosition() {

        Vector3d v = new Vector3d(getTranslationSpeed());
        Matrix3d r = new Matrix3d(getRotationSpeed());

        v.scale(getDt());
        
        final Vector3d newPos = getPosition();       
        Vector3d oldPos = new Vector3d(newPos);
        
        newPos.add(v);

        Matrix3d oldRot = new Matrix3d(r);
        
        // r.mul(dt);
        r.transform(getOrientation());
        
        positionUpdated(oldPos,oldRot,newPos,r);

    }

    abstract protected void positionUpdated(Vector3d oldPos, Matrix3d oldRot, Vector3d pos, Matrix3d rot);
}
